#include <assert.h>
#include <stdint.h>
#include <string.h>
#include <stdarg.h>
#include "pcan_usbpro_fw.h"
#include "pcanpro_usbd.h"
#include "pcanpro_timestamp.h"
#include "pcan_usb.h"
#include "pcanfd_ucan.h"
#include "pcanfd_usb_fw.h"
#include "pcanpro_can.h"

uint8_t usbReady = 0;

#define CAN_CHANNEL_MAX     (2)
extern FDCAN_HandleTypeDef hfdcan1;
extern FDCAN_HandleTypeDef hfdcan2;
int pcan_protocol_rx_frame( uint8_t channel, struct t_can_msg *pmsg );
 uint32_t LED1_Send_Sts = 9999;
 uint32_t LED2_Send_Sts = 9999;
struct pcan_usbfd_fw_info
{
  uint16_t  size_of;        /* sizeof this */
  uint16_t  type;            /* type of this structure */
  uint8_t    hw_type;        /* Type of hardware (HW_TYPE_xxx) */
  uint8_t    bl_version[3];  /* Bootloader version */
  uint8_t    hw_version;      /* Hardware version (PCB) */
  uint8_t    fw_version[3];  /* Firmware version */
  uint32_t  dev_id[2];      /* "device id" per CAN */
  uint32_t  ser_no;          /* S/N */
  uint32_t  flags;          /* special functions */
  uint8_t   unk[8];
} __attribute__ ((packed));

#define PCAN_USB_DATA_BUFFER_SIZE   2048
static uint8_t resp_buffer[2][PCAN_USB_DATA_BUFFER_SIZE];
static uint8_t drv_load_packet[16];

static uint16_t data_pos = 0;
static uint8_t   data_buffer[PCAN_USB_DATA_BUFFER_SIZE];

static struct t_m2h_fsm resp_fsm[2] = 
{
  [0] = {
    .state = 0,
    .ep_addr = PCAN_USB_EP_CMDIN,
    .pdbuf = resp_buffer[0],
    .dbsize = PCAN_USB_DATA_BUFFER_SIZE,
  },
  [1] = {
    .state = 0,
    .ep_addr = PCAN_USB_EP_MSGIN_CH1,
    .pdbuf = resp_buffer[1],
    .dbsize = PCAN_USB_DATA_BUFFER_SIZE,
  }
};

/* low level requests */
uint8_t pcan_protocol_device_setup( USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req )
{
  switch( req->bRequest )
  {
    case USB_VENDOR_REQUEST_INFO:
      switch( req->wValue )
      {
        case USB_VENDOR_REQUEST_wVALUE_INFO_FIRMWARE:
        {
          static struct pcan_usbfd_fw_info fwi =
          {
            .size_of = sizeof( struct pcan_usbfd_fw_info ),
            .type = 2,
            .hw_type = 1,
            .bl_version = { 2, 1, 0 }, /* bootloader v > 2 support massstorage mode */
            .hw_version = 2,
            .fw_version = { 3, 2, 0 },
            .dev_id[0] = 0xFFFFFFFF,
            .dev_id[1] = 0xFFFFFFFF,
            .ser_no = 0xFFFFFFFF,
            .flags = 0x00000000,
            .unk = { 
              0x01, /* cmd_out */
              0x81, /* cmd_in */
              0x02, /* write */
              0x03, /* write */
              0x82, /* read */
              0x00,
              0x00,
              0x00 
              }
          };
          /* windows/linux has different struct size */
          fwi.size_of = req->wLength;
          fwi.dev_id[0] = 0xFFFFFFFF;
          fwi.dev_id[1] = 0xFFFFFFFF;
          fwi.ser_no = 0xFFFFFFFF;
          return USBD_CtlSendData( pdev,  (void*)&fwi, fwi.size_of );
        }
        default:
          assert(0);
        break;
      }
      break;
    case USB_VENDOR_REQUEST_FKT:
      switch( req->wValue )
      {
        case USB_VENDOR_REQUEST_wVALUE_SETFKT_BOOT:
          break;
        case USB_VENDOR_REQUEST_wVALUE_SETFKT_INTERFACE_DRIVER_LOADED:
        {
          USBD_CtlPrepareRx( pdev, drv_load_packet, 16 );
          return USBD_OK;
        }
        break;
        default:
          assert(0);
          break;
      }
      break;
    case USB_VENDOR_REQUEST_ZERO:
      break;
    default:
      USBD_CtlError( pdev, req );
      return USBD_FAIL; 
  }

  return USBD_FAIL;
}

void pcan_ep0_receive( void )
{
  if( drv_load_packet[0] == 0 )
  {
    pcan_flush_ep( PCAN_USB_EP_MSGIN_CH1 );
    pcan_flush_ep( PCAN_USB_EP_CMDIN );
  }
}

void *pcan_data_alloc_buffer( uint16_t type, uint16_t size )
{
  uint16_t aligned_size = (size+(4-1))&(~(4-1));
  if( sizeof( data_buffer ) < (aligned_size+data_pos+4) )
    return (void*)0;
  struct ucan_msg *pmsg = (void*)&data_buffer[data_pos];

  pmsg->size = aligned_size;
  pmsg->type = type;
  pmsg->ts_low = pcan_timestamp_us();
  pmsg->ts_high = 0;

  data_pos += aligned_size;
  return pmsg;
}
uint32_t LED1_Sts = 9999,LED2_Sts = 9999;
//can�����ж�
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
  static FDCAN_RxHeaderTypeDef RxHeader;
  static struct t_can_msg RecMsg;
  static const uint8_t pcan_fd_dlc2len[] = 
  {
    0, 1, 2, 3, 4, 5, 6, 7,
    8, 12, 16, 20, 24, 32, 48, 64
  };

  memset(&RecMsg, 0, sizeof(struct t_can_msg));
  if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
  {
    /* Retrieve Rx messages from RX FIFO0 */
    if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RecMsg.data) == HAL_OK)
    {
		//Error_Handler();
		if(hfdcan == &hfdcan1) 
		{
			RecMsg.id = RxHeader.Identifier;
			if(RxHeader.BitRateSwitch == FDCAN_BRS_ON)
			{
				RecMsg.flags |= MSG_FLAG_BRS;
			}
			if(RxHeader.FDFormat == FDCAN_FD_CAN)
			{
				RecMsg.flags |= MSG_FLAG_FD;
			}
			if(RxHeader.IdType == FDCAN_EXTENDED_ID)
			{
				RecMsg.flags |= MSG_FLAG_EXT;
			}
			
			RecMsg.size = pcan_fd_dlc2len[RxHeader.DataLength];
			RecMsg.timestamp = pcan_timestamp_us();
			pcan_protocol_rx_frame(0, &RecMsg);
			//pcan_protocol_poll();
			LED1_Sts = 1;
		}
		else if(hfdcan == &hfdcan2) 
		{
			RecMsg.id = RxHeader.Identifier;
			if(RxHeader.BitRateSwitch == FDCAN_BRS_ON)
			{
				RecMsg.flags |= MSG_FLAG_BRS;
			}
			if(RxHeader.FDFormat == FDCAN_FD_CAN)
			{
				RecMsg.flags |= MSG_FLAG_FD;
			}
			if(RxHeader.IdType == FDCAN_EXTENDED_ID)
			{
				RecMsg.flags |= MSG_FLAG_EXT;
			}
			RecMsg.size = pcan_fd_dlc2len[RxHeader.DataLength];
			RecMsg.timestamp = pcan_timestamp_us();
			pcan_protocol_rx_frame(1, &RecMsg);
			LED2_Sts = 1;
			//pcan_protocol_poll();
		}
    }	
  }
}

void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo1ITs)
{
	static FDCAN_RxHeaderTypeDef RxHeader;
	static struct t_can_msg RecMsg;
	static const uint8_t pcan_fd_dlc2len[] = 
	{
	  0, 1, 2, 3, 4, 5, 6, 7,
	  8, 12, 16, 20, 24, 32, 48, 64
	};
	
	memset(&RecMsg, 0, sizeof(struct t_can_msg));
	if((RxFifo1ITs & FDCAN_IT_RX_FIFO1_NEW_MESSAGE) != RESET)
	{
	  /* Retrieve Rx messages from RX FIFO0 */
	  if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO1, &RxHeader, RecMsg.data) == HAL_OK)
	  {
		  //Error_Handler();
		  if(hfdcan == &hfdcan1) 
		  {
			  RecMsg.id = RxHeader.Identifier;
			  if(RxHeader.BitRateSwitch == FDCAN_BRS_ON)
			  {
				  RecMsg.flags |= MSG_FLAG_BRS;
			  }
			  if(RxHeader.FDFormat == FDCAN_FD_CAN)
			  {
				  RecMsg.flags |= MSG_FLAG_FD;
			  }
			  if(RxHeader.IdType == FDCAN_EXTENDED_ID)
			  {
				  RecMsg.flags |= MSG_FLAG_EXT;
			  }
			  
			  RecMsg.size = pcan_fd_dlc2len[RxHeader.DataLength];
			  RecMsg.timestamp = pcan_timestamp_us();
			  pcan_protocol_rx_frame(0, &RecMsg);
			  //pcan_protocol_poll();
			  LED1_Sts = 1;
		  }
		  else if(hfdcan == &hfdcan2) 
		  {
			  RecMsg.id = RxHeader.Identifier;
			  if(RxHeader.BitRateSwitch == FDCAN_BRS_ON)
			  {
				  RecMsg.flags |= MSG_FLAG_BRS;
			  }
			  if(RxHeader.FDFormat == FDCAN_FD_CAN)
			  {
				  RecMsg.flags |= MSG_FLAG_FD;
			  }
			  if(RxHeader.IdType == FDCAN_EXTENDED_ID)
			  {
				  RecMsg.flags |= MSG_FLAG_EXT;
			  }
			  RecMsg.size = pcan_fd_dlc2len[RxHeader.DataLength];
			  RecMsg.timestamp = pcan_timestamp_us();
			  pcan_protocol_rx_frame(1, &RecMsg);
			  LED2_Sts = 1;
			  //pcan_protocol_poll();
		  }
	  }   
	}

}

void PCANProgress(void)
{
	volatile static uint8_t aachn = 0;
	volatile static struct t_can_msg aamsg;
	volatile static uint32_t test1s = 0;
	volatile static uint16_t aatest = 1;

	if(aatest)
		aatest++;
	if(aatest >= 250)
	{
		aatest = 0;
		usbReady = 1;
		HAL_FDCAN_Start(&hfdcan1);
		HAL_FDCAN_Start(&hfdcan2);
		if(test1s == 0)
		{
			test1s = 1;
		}
	}
	
//	if(aatest >= 250)
//	{
//		aatest = 250-10;
//
//		aamsg.id = 0x123;
//		aamsg.flags = (MSG_FLAG_FD | MSG_FLAG_BRS);
//		aamsg.size = 64;
//		memcpy(aamsg.data, "1234567812345678123456781234567812345678123456781234567812345678", 64);
//		aamsg.timestamp = pcan_timestamp_us();
//		pcan_protocol_rx_frame(aachn, &aamsg);
//
//		aamsg.id = 0x124;
//		pcan_protocol_rx_frame(aachn, &aamsg);
//
//		aamsg.id = 0x125;
//		pcan_protocol_rx_frame(aachn, &aamsg);
//
//		aamsg.id = 0x126;
//		pcan_protocol_rx_frame(aachn, &aamsg);
//
//		aamsg.id = 0x127;
//		pcan_protocol_rx_frame(aachn, &aamsg);
//
//
//		pcan_protocol_poll();
//	}

	if(test1s)
	{
		test1s++;
		if(test1s >= 100)
		{
			test1s = 1;
			struct ucan_usb_ts_msg *pts = pcan_data_alloc_buffer( UCAN_USB_MSG_CALIBRATION, sizeof( struct ucan_usb_ts_msg ) );
			if( !pts )
				return;

			pts->usb_frame_index = 0;
			pts->unused = 0;
		
		}
	}



}


int pcan_protocol_rx_frame( uint8_t channel, struct t_can_msg *pmsg )
{
  static const uint8_t pcan_fd_len2dlc[] = 
  {
    0, 1, 2, 3, 4, 5, 6, 7, 8,	/* 0 - 8 */
    9, 9, 9, 9,	    /* 9 - 12 */
    10, 10, 10, 10,	    /* 13 - 16 */
    11, 11, 11, 11,	    /* 17 - 20 */
    12, 12, 12, 12,	    /* 21 - 24 */
    13, 13, 13, 13, 13, 13, 13, 13,	/* 25 - 32 */
    14, 14, 14, 14, 14, 14, 14, 14,	/* 33 - 40 */
    14, 14, 14, 14, 14, 14, 14, 14,	/* 41 - 48 */
    15, 15, 15, 15, 15, 15, 15, 15,	/* 49 - 56 */
    15, 15, 15, 15, 15, 15, 15, 15	/* 57 - 64 */
  };
  struct ucan_rx_msg *pcan_msg = pcan_data_alloc_buffer( UCAN_MSG_CAN_RX, sizeof(struct ucan_rx_msg) + pmsg->size );
  if( !pcan_msg )
    return -1;

  if( !(pmsg->flags & MSG_FLAG_ECHO) )
  {
//    uint32_t nqt = pcan_device.can[channel].nominal_qt;
//    uint32_t dqt = pcan_device.can[channel].data_qt;
//    pcan_device.can[channel].rx_time_ns += pcan_can_msg_time( pmsg, nqt, dqt );
//    if( !pcan_device.can[channel].led_is_busy )
//    {
//      pcan_led_set_mode( channel ? LED_CH1_RX:LED_CH0_RX, LED_MODE_BLINK_FAST, 237 );
//    }
  }

  if( pmsg->size > CAN_PAYLOAD_MAX_SIZE )
    pmsg->size = CAN_PAYLOAD_MAX_SIZE;
  pcan_msg->channel_dlc = UCAN_MSG_CHANNEL_DLC( channel, pcan_fd_len2dlc[pmsg->size] );
  pcan_msg->client = pmsg->dummy;
  pcan_msg->flags = 0;
  pcan_msg->tag_low = 0;
  pcan_msg->tag_high = 0;

  /* we support only regular frames */
  if( pmsg->flags & MSG_FLAG_RTR )
    pcan_msg->flags |= UCAN_MSG_RTR;
  if( pmsg->flags & MSG_FLAG_EXT )
    pcan_msg->flags |= UCAN_MSG_EXT_ID;
  if( pmsg->flags & MSG_FLAG_FD )
  {
    pcan_msg->flags |= UCAN_MSG_EXT_DATA_LEN;
    if( pmsg->flags & MSG_FLAG_BRS )
      pcan_msg->flags |= UCAN_MSG_BITRATE_SWITCH;
    if( pmsg->flags & MSG_FLAG_ESI )
      pcan_msg->flags |= UCAN_MSG_ERROR_STATE_IND;
  }
  if( pmsg->flags & MSG_FLAG_ECHO )
  {
    pcan_msg->flags |= UCAN_MSG_API_SRR | UCAN_MSG_HW_SRR;
    pcan_msg->ts_low = pcan_timestamp_us();
  }
  else
  {
    pcan_msg->ts_low = pmsg->timestamp;
  }

  pcan_msg->can_id = pmsg->id;
  memcpy( pcan_msg->d, pmsg->data, pmsg->size );
  return 0;
}

int pcan_protocol_tx_frame_cb( uint8_t channel, struct t_can_msg *pmsg )
{
  return 0;
}

void FDCAN_Send_Msg(uint8_t chn, struct t_can_msg msg)
{	
  extern FDCAN_TxHeaderTypeDef TxHeader;
  FDCAN_HandleTypeDef* hfdcan;

  //memset(&TxHeader, 0, sizeof(FDCAN_TxHeaderTypeDef));
  
  TxHeader.Identifier = msg.id;
  if(msg.flags & MSG_FLAG_EXT)
  {
	  TxHeader.IdType = FDCAN_EXTENDED_ID;
  }
  else
  {
	  TxHeader.IdType = FDCAN_STANDARD_ID;
  }
  TxHeader.TxFrameType = FDCAN_DATA_FRAME;
  TxHeader.DataLength = msg.size;
  TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
  if(msg.flags & MSG_FLAG_FD)
  {
	  TxHeader.FDFormat = FDCAN_FD_CAN;
  }
  else
  {
	  TxHeader.FDFormat = FDCAN_CLASSIC_CAN;
  }

  if(msg.flags & MSG_FLAG_BRS)
  {
	  TxHeader.BitRateSwitch = FDCAN_BRS_ON;
  }
  else
  {
	  TxHeader.BitRateSwitch = FDCAN_BRS_OFF;
  }
  
  TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
  TxHeader.MessageMarker = 0;                    

  if(chn == 0)
  {
	hfdcan = &hfdcan1;
	LED1_Send_Sts = 1;
  }
  else
  {
	hfdcan = &hfdcan2;
	LED2_Send_Sts = 1;
  }
  HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &TxHeader, msg.data);
}

int pcan_protocol_tx_frame( struct ucan_tx_msg *pmsg )
{
  static const uint8_t pcan_fd_dlc2len[] = 
  {
    0, 1, 2, 3, 4, 5, 6, 7,
    8, 12, 16, 20, 24, 32, 48, 64
  };

  struct t_can_msg msg = { 0 };
  uint8_t channel;
  uint8_t len;

  channel = UCAN_MSG_CHANNEL(pmsg);

  if( channel >= CAN_CHANNEL_MAX )
    return -1;

  msg.id = pmsg->can_id;

  /* CAN-FD frame */
  if( pmsg->flags & UCAN_MSG_EXT_DATA_LEN )
  {
    msg.flags |= MSG_FLAG_FD;
    if( pmsg->flags & UCAN_MSG_BITRATE_SWITCH )
      msg.flags |= MSG_FLAG_BRS;
    if( pmsg->flags & UCAN_MSG_ERROR_STATE_IND )
      msg.flags |= MSG_FLAG_ESI;
    
    msg.size = UCAN_MSG_DLC(pmsg);//pcan_fd_dlc2len[UCAN_MSG_DLC(pmsg)];
    len = pcan_fd_dlc2len[UCAN_MSG_DLC(pmsg)];
  }
  else
  {
    msg.size = UCAN_MSG_DLC(pmsg);
	len = msg.size;
  }

  if( msg.size > sizeof( msg.data ) )
    return -1;

  /* TODO: process UCAN_MSG_SINGLE_SHOT, UCAN_MSG_HW_SRR, UCAN_MSG_ERROR_STATE_IND */
  if( pmsg->flags & UCAN_MSG_RTR )
    msg.flags |= MSG_FLAG_RTR;
  if( pmsg->flags & UCAN_MSG_EXT_ID )
    msg.flags |= MSG_FLAG_EXT;
  if( pmsg->flags & (UCAN_MSG_API_SRR|UCAN_MSG_HW_SRR) )
  {
    msg.flags |= MSG_FLAG_ECHO;
    msg.dummy = pmsg->client;
  }

  memcpy( msg.data, pmsg->d, len );

  FDCAN_Send_Msg(channel, msg);

//  msg.timestamp = pcan_timestamp_us();
//
//  if( pcan_can_write( channel, &msg ) < 0 )
//  {
//    /* TODO: tx queue overflow ? */
//    ;
//  }

  return 0;
}

static int _get_precalculated_bitrate( uint32_t bitrate, uint32_t *brp, uint32_t *tseg1, uint32_t *tseg2)
{
  //144MHZ
  switch( bitrate )
  {
    case 8000000u://18
      *brp = 2;
      *tseg1 = 6;
      *tseg2 = 2;
    break;
    case 6000000u:
      *brp = 2;
      *tseg1 = 8;
      *tseg2 = 3;
    break;
    case 4000000u:
      *brp = 4;
      *tseg1 = 6;
      *tseg2 = 2;
    break;
    case 2000000u:
      *brp = 6;
      *tseg1 = 8;
      *tseg2 = 3;
    break;
    case 1000000u:
      *brp = 6;
      *tseg1 = 17;
      *tseg2 = 6;
    break;
    case 800000u:
      *brp = 36;
      *tseg1 = 3;
      *tseg2 = 1;
    break;
    case 500000u:
      *brp = 18;
      *tseg1 = 12;
      *tseg2 = 3;
    break;
    case 250000u:
      *brp = 6*6;
      *tseg1 = 13;
      *tseg2 = 2;
    break;
    case 125000u:
      *brp = 12*6;
      *tseg1 = 13;
      *tseg2 = 2;
    break;
    case 100000u:
      *brp = 15*6;
      *tseg1 = 13;
      *tseg2 = 2;
    break;
    case 50000u:
      *brp = 30*6;
      *tseg1 = 13;
      *tseg2 = 2;
    break;
    case 20000u:
      *brp = 75*6;
      *tseg1 = 13;
      *tseg2 = 2;
    break;

    default:
		return 1;
    	break;
  }

  return 0;
}

int pcan_protocol_set_baudrate( uint8_t channel, uint32_t FreqMHZ, struct t_can_bitrate *pbitrate, struct t_can_bitrate *pdata_bitrate )
{

  uint32_t brp, tseg1, tseg2, sjw;
  uint32_t freq;
  uint8_t ret;

  if(pbitrate != NULL)
  {
	  freq = FreqMHZ/pbitrate->brp/(1+pbitrate->tseg1+pbitrate->tseg2);
  }
  
  if(pdata_bitrate != NULL)
  {
	  freq = FreqMHZ/pdata_bitrate->brp/(1+pdata_bitrate->tseg1+pdata_bitrate->tseg2);
  }
  
  ret = _get_precalculated_bitrate( freq, &brp, &tseg1, &tseg2);
  if(ret == 1)
  {
		return 1;
  }
  
  if(channel == 0)
  {
	  if(pbitrate != NULL)
	  {
		  hfdcan1.Init.NominalPrescaler = brp;
		  hfdcan1.Init.NominalTimeSeg1 = tseg1;
		  hfdcan1.Init.NominalTimeSeg2 = tseg2;
		  hfdcan1.Init.NominalSyncJumpWidth = pbitrate->sjw;
	  }

	  if(pdata_bitrate != NULL)
	  {
		  hfdcan1.Init.DataPrescaler = brp;
		  hfdcan1.Init.DataTimeSeg1 = tseg1;
		  hfdcan1.Init.DataTimeSeg2 = tseg2;
		  hfdcan1.Init.DataSyncJumpWidth = pdata_bitrate->sjw;
	  }
	  HAL_FDCAN_Init(&hfdcan1);	
	  FDCAN1_config();
	  HAL_FDCAN_Start(&hfdcan1);
  }
  else if(channel == 1)
  {
  	  if(pbitrate != NULL)
	  {
		  hfdcan2.Init.NominalPrescaler = brp;
		  hfdcan2.Init.NominalTimeSeg1 = tseg1;
		  hfdcan2.Init.NominalTimeSeg2 = tseg2;
		  hfdcan2.Init.NominalSyncJumpWidth = pbitrate->sjw;
	  }

	  if(pdata_bitrate != NULL)
	  {
		  hfdcan2.Init.DataPrescaler = brp;
		  hfdcan2.Init.DataTimeSeg1 = tseg1;
		  hfdcan2.Init.DataTimeSeg2 = tseg2;
		  hfdcan2.Init.DataSyncJumpWidth = pdata_bitrate->sjw;
	  }
	  HAL_FDCAN_Init(&hfdcan2);
	  FDCAN2_config();
	  HAL_FDCAN_Start(&hfdcan2);
  }
  
}

static void pcan_protocol_process_cmd( uint8_t *ptr, uint16_t size )
{
  struct ucan_command *pcmd = (void*)ptr;
  volatile static uint32_t FreqMHZ = 0;

  while( size >= sizeof( struct ucan_command ) )
  {
    switch( UCAN_CMD_OPCODE( pcmd ) )
    {
      case UCAN_CMD_NOP:
        break;
      case UCAN_CMD_RESET_MODE:
      {
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
          //pcan_can_set_bus_active( UCAN_CMD_CHANNEL(pcmd) , 0 );
          //pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].bus_active = 0;

          /* update ISO mode only on inactive bus */
          //uint8_t bus_iso_mode = ( pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].opt_mask & UCAN_OPTION_ISO_MODE ) != 0;
          //pcan_can_set_iso_mode( UCAN_CMD_CHANNEL(pcmd), bus_iso_mode );
        }
      }
        break;
      case UCAN_CMD_NORMAL_MODE:
      {
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
//          if( pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].silient  )
//          {
//            pcan_can_set_silent( UCAN_CMD_CHANNEL(pcmd) , 0 );
//            pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].silient = 0;
//          }
//          pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].bus_active = 1;
//          pcan_can_set_bus_active( UCAN_CMD_CHANNEL(pcmd) , 1 );
//          pcan_protocol_send_status( UCAN_CMD_CHANNEL(pcmd), 0 );
        }
      }
        break;
      case UCAN_CMD_LISTEN_ONLY_MODE:
      {
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
//          pcan_can_set_silent( UCAN_CMD_CHANNEL(pcmd) , 1 );
//          pcan_can_set_bus_active( UCAN_CMD_CHANNEL(pcmd) , 1 );
//          pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].silient = 1;
//          pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].bus_active = 1;
//          pcan_protocol_send_status( UCAN_CMD_CHANNEL(pcmd), 0 );
        }
      }
        break;
      case UCAN_CMD_TIMING_SLOW:
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
          struct t_can_bitrate br;
          struct ucan_timing_slow *ptiming = (void*)pcmd;
          //pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].slow_br = *ptiming;
          
          br.brp = ptiming->brp + 1;
          br.tseg1 = ptiming->tseg1 + 1;
          br.tseg2 = ptiming->tseg2 + 1;
          br.sjw = (ptiming->sjw_t & 0x0f)+1;
          (void)((ptiming->sjw_t&0x80) != 0);

          pcan_protocol_set_baudrate( UCAN_CMD_CHANNEL(pcmd), FreqMHZ, &br, NULL );
        }
        break;
      /* only for CAN-FD */
      case UCAN_CMD_TIMING_FAST:
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
          volatile static struct t_can_bitrate br;
          struct ucan_timing_fast *ptiming = (void*)pcmd;
          //pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].fast_br = *ptiming;

          br.brp = ptiming->brp + 1;
          br.tseg1 = ptiming->tseg1 + 1;
          br.tseg2 = ptiming->tseg2 + 1;
          br.sjw = (ptiming->sjw & 0x0f)+1;
          
          pcan_protocol_set_baudrate( UCAN_CMD_CHANNEL(pcmd), FreqMHZ, NULL, &br );
        }
        break;
      case UCAN_CMD_SET_STD_FILTER:
        if( UCAN_CMD_CHANNEL(pcmd) >= CAN_CHANNEL_MAX )
          break;
        //pcan_can_set_filter_mask( UCAN_CMD_CHANNEL(pcmd), 0, 0, 0, 0 );
        break;
      case UCAN_CMD_RESERVED2:
        break;
      case UCAN_CMD_FILTER_STD:
        if( UCAN_CMD_CHANNEL(pcmd) >= CAN_CHANNEL_MAX )
          break;
        //pcan_can_set_filter_mask( UCAN_CMD_CHANNEL(pcmd), 0, 0, 0, 0 );
        break;
      case UCAN_CMD_TX_ABORT:
        break;
      case UCAN_CMD_WR_ERR_CNT:
        break;
      case UCAN_CMD_SET_EN_OPTION:
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
          struct ucan_option *popt = (void*)pcmd;
          //pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].opt_mask |= popt->mask;
        }
        break;
      case UCAN_CMD_CLR_DIS_OPTION:
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
          struct ucan_option *popt = (void*)pcmd;
          //pcan_device.can[UCAN_CMD_CHANNEL(pcmd)].opt_mask &= ~popt->mask;
        }
        break;
      case UCAN_CMD_SET_ERR_GEN1:
        break;
      case UCAN_CMD_SET_ERR_GEN2:
        break;
      case UCAN_CMD_DIS_ERR_GEN:
        break;
      case UCAN_CMD_RX_BARRIER:
        break;
      case UCAN_CMD_SET_ERR_GEN_S:
        break;
      case UCAN_USB_CMD_CLK_SET:
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
            volatile static struct ucan_usb_clock *pclock;
            pclock = (void*)pcmd;
            switch( pclock->mode )
            {
              default:
              case UCAN_USB_CLK_80MHZ:
            	  FreqMHZ = 80000000u;
              break;
              case UCAN_USB_CLK_60MHZ:
            	  FreqMHZ = 60000000u;
              break;
              case UCAN_USB_CLK_40MHZ:
            	  FreqMHZ = 40000000u;
              break;
              case UCAN_USB_CLK_30MHZ:
            	  FreqMHZ = 30000000u;
              break;
              case UCAN_USB_CLK_24MHZ:
            	  FreqMHZ = 24000000u;
              break;
              case UCAN_USB_CLK_20MHZ:
            	  FreqMHZ = 20000000u;
              break;
            }
        }
        break;
      case UCAN_USB_CMD_LED_SET:
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
          struct ucan_usb_led *pled = (void*)pcmd;
        }
        break;
      case UCAN_USB_CMD_DEVID_SET:
      {
        struct ucan_usb_device_id *pdevid = (void*)pcmd;
        if( UCAN_CMD_CHANNEL(pcmd) < CAN_CHANNEL_MAX )
        {
        }
        break;
      }
      case 0x87: /* CAN FD ISO MODE, 0xff - enable, 0x55 - disable  */
      {
        /* do nothing here */
        break;
      }
      case UCAN_CMD_END_OF_COLLECTION:
        return;
      default:
        assert( 0 );
        break;
    }

    size -= sizeof( struct ucan_command );
    ++pcmd;
  }
}

void pcan_protocol_process_data( uint8_t ep, uint8_t *ptr, uint16_t size )
{
  if( ep == 1 )
  {
    pcan_protocol_process_cmd( ptr, size );
    return;
  }
  /* message data ? */
  struct ucan_msg *pmsg = 0;

  while( size )
  {
    if( size < 4 )
      break;
    pmsg = (void*)ptr;
    if( !pmsg->size || !pmsg->type )
      break;
    if( size < pmsg->size )
      break;
    size -= pmsg->size;
    ptr += pmsg->size;

    switch( pmsg->type )
    {
      //to host only
      //UCAN_MSG_ERROR:
      //UCAN_MSG_BUSLOAD:
      case UCAN_MSG_CAN_TX:
        pcan_protocol_tx_frame( (struct ucan_tx_msg *)pmsg );
        break;
      case UCAN_MSG_CAN_TX_PAUSE:
        /* TODO: */
        break;
      case UCAN_CMD_END_OF_COLLECTION:
      case 0xffff:
        return;
      default:
        assert( 0 );
        break;  
    }
  }
}


char testsend = 0;
void pcan_protocol_poll( void )
{
  uint32_t ts_ms = pcan_timestamp_millis();
  uint32_t ts_us = pcan_timestamp_us();

  //pcan_can_poll();

  /* flush data */
  if( data_pos > 0 )
  {
    /* endmark */
    *(uint32_t*)&data_buffer[data_pos] = 0x00000000;
    uint16_t flush_size = data_pos + 4;
    /* align to 64 */
    flush_size += (64-1);
    flush_size &= ~(64-1);
	testsend = 1;
    int res = pcan_flush_data( &resp_fsm[1], data_buffer, data_pos );
    if( res )
    { 
      data_pos = 0;
      //pcan_device.last_time_flush = ts_us;
    }
  }
}
